/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.vision.Target;
import edu.wpi.first.wpilibj.vision.TrackerDashboard;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Victor;

public class RobotMain extends SimpleRobot {

    final int Preload = 1;
    final int Retract = 0;
    final int Latch = 1;
    final int Unlatch = 0;
    final int Left = 1;
    final int Right = 0;
    final int Forward = 1;
    final int Reverse = -1;
    final int Off = 0;
    boolean ballPresent = false;
    boolean broken = false;
    private final double kVisionThreshold = .01;
    private Compressor compressor = new Compressor(4, 14, 4, 1);
    private Joystick leftStick = new Joystick(1);
    private Joystick rightStick = new Joystick(2);
    private Team997Drive drivetrain = new Team997Drive(1, 2, 3, 4);
    private AxisCamera cam;
    // Camera isn't working (2/6/2010)
    // May want to disable camera if the connection isn't working well.
    // A bad camera connection can cause the Robot to hang for
    // approximately 40 seconds.  
    private final boolean enableCamera = true;
    private TrackerDashboard trackerDashboard = new TrackerDashboard();
    private Gyro gyro = new Gyro(1);
    private final double gyroKP = 0.03;
    private DigitalInput autobit[] = new DigitalInput[4];
    private DigitalInput breakBeam = new DigitalInput(4, 5);//1 if broken
    private Victor ballHolder = new Victor(5);
    private Solenoid fireShooterSolenoid = new Solenoid(8, 1);
    //Solenoids hardware switched to single control wire.
    private Solenoid retractShooterSolenoid = new Solenoid(8, 2);
    private Solenoid latchSolenoid = new Solenoid(8, 3);
    //Solenoids hardware switched to single control wire.
    private Solenoid unlatchSolenoid = new Solenoid(8, 4);
    private boolean preloaded = false;
    private boolean latched = false;
    private boolean mLifterRaised = false;
    private boolean mLifterExtended = false;
    private boolean mBallHolderOn = false;
    private Encoder leftEncoder = new Encoder(4, 12, 4, 13, false, Encoder.EncodingType.k2X);
    private Encoder rightEncoder = new Encoder(4, 10, 4, 11, false, Encoder.EncodingType.k2X);

    public RobotMain() {
        getWatchdog().setEnabled(false);
        gyro.reset();
        gyro.setSensitivity(0.0047);
        leftEncoder.setDistancePerPulse(0.02);
        leftEncoder.start();
        leftEncoder.reset();
        rightEncoder.setDistancePerPulse(0.02); // shou;d be 48 / 4000 won't auto calculate
        rightEncoder.start();
        rightEncoder.reset();
        if (enableCamera) {
            cam = AxisCamera.getInstance();
            cam.writeResolution(AxisCamera.ResolutionT.k320x240);
            cam.writeBrightness(0);
        }
        compressor.start();//
        autobit[0] = new DigitalInput(1);
        autobit[1] = new DigitalInput(2);
        autobit[2] = new DigitalInput(3);
        autobit[3] = new DigitalInput(4);
        //breakBeam[0] = new DigitalInput(8);
        //getWatchdog().feed();

        // We start being not preloaded and unlatched.
        fireShooterSolenoid.set(false);
        retractShooterSolenoid.set(true);
        unlatchSolenoid.set(true);
        latchSolenoid.set(false);
        leftEncoder.reset();
        rightEncoder.reset();
    }

    /**
     * Performs the autonomous.  Turns off the watchdog,
     * starts a seperate kicker thread for autonomous, gets the zone number,
     * and starts the specified zone's autonomous routine.
     */
    public void autonomous() {
        // Disable watchdog while in Autonomous.

        getWatchdog().setEnabled(false);
        AutoKickerThread autoKickerThread = new AutoKickerThread(this);
        autoKickerThread.start();
        int zone = currentZone();
        switchBallHolder(Forward);
        // We get ready for the first kick as step 1 in the zone.
        switch (zone) {
            case 1:
                zone1_1Autonomous(autoKickerThread);
                break;
            case 2:
                zone2_2Autonomous(autoKickerThread);
                break;
            case 3:
                zone3_3Autonomous(autoKickerThread);
                break;
            case 4:
                zone1_4Autonomous(autoKickerThread);
                break;
            case 5:
                zone2_5Autonomous(autoKickerThread);
                break;
            case 6:
                zone3_6Autonomous(autoKickerThread);
                break;
            default:
                break;
        }
        try {
            autoKickerThread.join();
        } catch (InterruptedException ex) {
            System.out.println(ex.getMessage());
        }

    }

    /**
     * Gets the zone according to the dial on the robot.
     * @return The zone specified on the dial.
     */
    private int currentZone() {
        int zone = 0;
        for (int i = 0; i < 4; i++) {
            if (!autobit[i].get()) {
                zone = zone + pow(2, i);
            }
        }
        return zone;
    }

    /**
     * Performs the power function.
     * @param base The base
     * @param power The power
     * @return The base raised to the power.
     */
    public int pow(int base, int power) {
        int result = 1;
        if (power != 0) {
            for (int i = 0; i < power; i++) {
                result = result * base;
            }
        }
        return result;
    }

    /**
     * Attemts to kick 1 ball in zone 1.
     * @param autoKickerThread The sperate thread for kicking.
     */
    private void zone1_1Autonomous(AutoKickerThread autoKickerThread) {
        makeReadyForSoftKick();
        //Drive Forward to ball.
        goToBall(95);      //Travel to farthest ball placement
        autoKickerThread.autoSoftKick();

        //Position for Teleop
        turn(90, Right);
        moveInches(36, Forward);

    }

    /**
     * Attemts to kick 2 balls in zone 2.
     * @param autoKickerThread The sperate thread for kicking.
     */
    private void zone2_2Autonomous(AutoKickerThread autoKickerThread) {
        makeReadyForKick();
        goToBall(21);
        autoKickerThread.autoKick();

        goToBall(38);
        autoKickerThread.autoKick();

        turn(90, Right);
        moveInches(20, Forward);
    }

    /**
     * Attemts to kick 3 balls in zone 3.
     * @param autoKickerThread The sperate thread for kicking.
     */
    private void zone3_3Autonomous(AutoKickerThread autoKickerThread) {
        makeReadyForKick();
        goToBall(36);
        autoKickerThread.autoKick();

        goToBall(38);
        autoKickerThread.autoKick();

        goToBall(38);
        autoKickerThread.autoKick();

        turn(90, Right);
        moveInches(20, Forward);
    }

    private void zone1_4Autonomous(AutoKickerThread autoKickerThread) {
        makeReadyForSoftKick();
        moveInches(93, Forward);      //Travel to farthest ball placement
        autoKickerThread.autoSoftKick();

        //Position for Teleop
        turn(90, Right);
        moveInches(36, Forward);
    }

    private void zone2_5Autonomous(AutoKickerThread autoKickerThread) {
        makeReadyForKick();
        moveInches(19, Forward);
        autoKickerThread.autoKick();

        //We can kick over the balls in front, so just drive to straight to next ball.

        moveInches(36, Forward);
        autoKickerThread.autoKick();

        turn(90, Right);
        moveInches(20, Forward);
    }

    private void zone3_6Autonomous(AutoKickerThread autoKickerThread) {
        makeReadyForKick();
        moveInches(33.5, Forward);
        autoKickerThread.autoKick();

        //We can kick over the balls in front, so just drive to straight to next ball.

        moveInches(36, Forward);
        autoKickerThread.autoKick();

        moveInches(36, Forward);
        autoKickerThread.autoKick();

        turn(90, Right);
        moveInches(20, Forward);
    }

    /**
     * Setups the button thread for performing the kick operations, then allows
     * control of the motors in this thread.  Feeds the watchdog.
     */
    public void operatorControl() {
        getWatchdog().setEnabled(true);
        System.out.println(autobit[0].get() + " = output 1");
        System.out.println(autobit[1].get() + " = ouput 2");
        System.out.println(autobit[2].get() + " = output 3");
        System.out.println(autobit[3].get() + " = ouput 4");
        System.out.println(currentZone() + " = Zone");
        buttonThread buttonThread = new buttonThread(this);
        buttonThread.start();
        while (true && isOperatorControl() && isEnabled()) {
            //Dangerous code.  Warn people when this isn't commented out.
//            if (rightStick.getRawButton(9)) {
//            }
//            if (rightStick.getRawButton(3)) {
//                goToBall(36);
//                performKick();
//            }
            if (broken != breakBeam.get()) {
                broken = breakBeam.get();
                System.out.println(broken);
            }
            if (leftStick.getTrigger()) {
                pointAtVisionTarget();
            }
            drivetrain.tankDrive(getLeftStick(), getRightStick());
            getWatchdog().feed();
        }
        try {
            buttonThread.join();
        } catch (InterruptedException ex) {
            System.out.println(ex.getMessage());
        }
    }

    /**
     * Gets the left joystick
     * @return The left joystick
     */
    public Joystick getLeftStick() {
        return leftStick;
    }

    /**
     * Gets the right joystick
     * @return The right joystick
     */
    public Joystick getRightStick() {
        return rightStick;
    }

    /** 
     * Gets the robot ready for the big kick.
     */
    private void makeReadyForKick(){
        //Not latched and needs to be preloaded.
        if ((preloaded == false) && (latched == false)) {
            driveLatchSolenoid(Latch);
            Timer.delay(1);
            driveShooterSolenoid(Preload);
            Timer.delay(1);
        }
        //Latched, but needs to preload.
        if ((preloaded == false) && (latched == true)) {
            driveShooterSolenoid(Preload);
            Timer.delay(1);
        }
        //Preloaded, but needs to latch.
        if ((preloaded == true) && (latched == false)) {
            driveShooterSolenoid(Retract);
            Timer.delay(2);
            driveLatchSolenoid(Latch);
            Timer.delay(1);
            driveShooterSolenoid(Preload);
            Timer.delay(1);
        }
    }
    /**
     * Peforms the kick routine.  First loads the kicker if needed.
     * After kicking it loads the kicker for the next big kick.
     */
    public void performKick() {
        makeReadyForKick();
        //Kick!
        driveLatchSolenoid(Unlatch);
        Timer.delay(.5); // If this changed, change the delay in autoKick!!!
        //Reload for big kick.
        driveShooterSolenoid(Retract);
        Timer.delay(2);
        driveLatchSolenoid(Latch);
        Timer.delay(1);
        driveShooterSolenoid(Preload);
        System.out.println("Kick Performed");
    }

    /**
     * Gets the robot ready to perform a soft kick.
     */
    private void makeReadyForSoftKick(){
        //Loaded and latched for big kick, so let's release it.
        if ((preloaded == true) && (latched == true)) {
            driveShooterSolenoid(Retract);
            Timer.delay(1); // If this changed, change the delay in autoSoftKick!!!
            driveLatchSolenoid(Unlatch);
            Timer.delay(.5); // If this changed, change the delay in autoSoftKick!!!
        }
        //Latched, but not loaded.
        if ((preloaded == false) && (latched == true)) {
            driveLatchSolenoid(Unlatch);
            Timer.delay(.5);
        }
        //Preloaded and needs to be brought back.
        if ((preloaded == true) && (latched == false)) {
            driveShooterSolenoid(Retract);
            Timer.delay(2);
        }
    }

    /**
     * Performs a soft kick by releasing the depessurizing the kicker,
     * unlatching, and the repressurizing the unlatched kicker.
     */
    public void performSoftKick() {
        makeReadyForSoftKick();
        //Kick!
        driveShooterSolenoid(Preload);
        Timer.delay(.5); // If this changed, change the delay in autoSoftKick!!!
        //Reload for big kick
        driveShooterSolenoid(Retract);
        Timer.delay(2);
        // Do we really want to do this? Or should we comment this out and assume
        // we are in zone 1 so the next kick will be a soft one?
        driveLatchSolenoid(Latch);
        Timer.delay(1);
        driveShooterSolenoid(Preload);
        Timer.delay(1);
    }

    public void movePulse(int pulseNumber, int direction) {
        leftEncoder.reset();
        rightEncoder.reset();
        double angle;
        gyro.reset();
        System.out.println("Encoder Before: " + leftEncoder.get());
        System.out.println("Raw Before: " + leftEncoder.getRaw());
        if (direction == Forward) {
            //drivetrain.setLeftRightMotorSpeeds(-.5, -.5);
            while (leftEncoder.get() < pulseNumber) {
                angle = gyro.getAngle();
                drivetrain.drive(-.5, -angle * gyroKP);
                Thread.yield();
                // getWatchdog().feed();
            }
        } else {
            //drivetrain.setLeftRightMotorSpeeds(.5, .5);
            while (leftEncoder.get() > pulseNumber) {
                angle = gyro.getAngle();
                // getWatchdog().feed();
                drivetrain.drive(.5, -angle * gyroKP);
                Thread.yield();
            }
        }
        drivetrain.drive(0, 0);
        System.out.println("Encoder After: " + leftEncoder.get());
        System.out.println("Raw After: " + leftEncoder.getRaw());
    }


    /**
     * Moves the robot straight the specified number of inches.  Calculates it
     * based on the known linear relationship between pulse and inches.
     * @param inches Number of inches.
     * @param direction Direction of travel.
     */
    public void moveInches(double inches, int direction) {
        int pulse;
        if (inches > 6) {
            pulse = convertInchesToEncoderPulses(inches);
            movePulse(pulse, direction);
        }
    }

    /**
     * Calculates the number of encoder pulses required to travel a specified
     * distance.
     * @param inches Number of inches.
     * @return Number of encoder pulses to travel the specified distance.
     */
    private int convertInchesToEncoderPulses(double inches){
        return (int) ((inches - 6) / .05);
    }

    /**
     * Drives straight a specified distance
     * @param distanceInches Distance in inches.
     * @param direction Forward or backward.
     */
    public void goToBall(double maxDistanceInches) {
        int maxDistancePulse = 0;
        if (maxDistanceInches > 6) {
            maxDistancePulse = convertInchesToEncoderPulses(maxDistanceInches);
        }
        gyro.reset();
        double angle;
        leftEncoder.reset();
        boolean breakFlag = false;
        while (!breakBeam.get() && !breakFlag && (leftEncoder.get() < maxDistancePulse)) {
            angle = gyro.getAngle();
            drivetrain.drive(-.5, -angle * gyroKP);
            if (breakBeam.get()) {
                breakFlag = true;
            }
        }
        drivetrain.drive(0, 0);
    }

    public void move(int distanceInches, int direction) {
        System.out.println("Move" + distanceInches + direction);
        gyro.reset();
        leftEncoder.reset();
        rightEncoder.reset();
        double angle;
        System.out.println(leftEncoder.getDistance());
        if (direction == Forward) {
            while (leftEncoder.getDistance() < distanceInches) {
                //getWatchdog().feed();
                angle = gyro.getAngle();
                drivetrain.drive(-1.0, -angle * gyroKP);
                Timer.delay(0.004);
            }
        } else {
            while (leftEncoder.getDistance() > distanceInches) {
                angle = gyro.getAngle();
                // getWatchdog().feed();
                drivetrain.drive(1.0, -angle * gyroKP);
                Timer.delay(0.004);
            }
        }
        System.out.println(leftEncoder.getDistance());
        drivetrain.setLeftRightMotorSpeeds(0, 0);
    }

    /**
     * Rotates the robot in the specified direction
     * @param angle Angle of rotation
     * @param direction left or right
     */
    public void turn(double angle, int direction) { // successive turns fail
        gyro.reset();
        System.out.println("Turn" + angle + direction);
        //double initialAngle = gyro.getAngle();
        if (direction == Left) {
            drivetrain.setLeftRightMotorSpeeds(.2, -.2);
            while (gyro.getAngle() > (-angle)) {
                // getWatchdog().feed();
            }
        } else {
            drivetrain.setLeftRightMotorSpeeds(-.2, .2);
            while (gyro.getAngle() < (angle)) {
                // getWatchdog().feed();
            }
        }
        drivetrain.setLeftRightMotorSpeeds(0, 0);
        gyro.reset();
    }

    /**
     * Drives the shooter solenoid
     * @param direction 1 preloaded 0 flush
     */
    public void driveShooterSolenoid(int direction) {
        if (direction == 1) {
            retractShooterSolenoid.set(false);
            fireShooterSolenoid.set(true);
            preloaded = true;
        } else {
            fireShooterSolenoid.set(false);
            retractShooterSolenoid.set(true);
            preloaded = false;
        }
    }

    /**
     * Drives the latch solenoid.
     * @param direction 1 latched 0 retract
     */
    public void driveLatchSolenoid(int direction) {
        if (direction == 1) {
            unlatchSolenoid.set(false);
            latchSolenoid.set(true);
            latched = true;
        } else {
            latchSolenoid.set(false);
            unlatchSolenoid.set(true);
            latched = false;
        }
    }

    /**
     * Toggles the ball holder on and off.
     */
    public void switchBallHolder(int direction) {
        if (direction == Forward) {
            ballHolder.set(-1.0);
            System.out.println("Ball Holder Forward");
        } else if (direction == Off) {
            System.out.println("Ball Holder Off");
            ballHolder.set(0);
            mBallHolderOn = false;
        } else {
            ballHolder.set(1.0);
            System.out.println("Ball Holder Reverse");
        }
    }

    /**
     * Raises the lifter.
     */
    public void raiseLifter() {
        mLifterRaised = true;
        System.out.println("Lifter Raised");
    }

    /**
     * Extends the lifter.
     */
    public void extendLifter() {
        if (mLifterRaised) {
            mLifterExtended = true;
            System.out.println("Lifter Extended");
        }

    }

    /**
     * Performs the lift
     */
    public void performLift() {
        if (mLifterExtended) {
            System.out.println("Lift Performed");
        }
    }

    /**
     * Rotates the robot to point at the vision target, but only
     * if the target is in the camera's vision.
     */
    public void pointAtVisionTarget() {
        //Only do this if the camera is enabled.
        if (enableCamera) {
            drivetrain.tankDrive(0, 0);
            try {
                if (cam.freshImage()) {// && turnController.onTarget()) {
                    double gyroAngle = gyro.getAngle();
                    double targetAngle;
                    double gyroFinalAngle;
                    ColorImage image = cam.getImage();
                    getWatchdog().feed();
                    Thread.yield();
                    Target[] targets = Target.findCircularTargets(image);
                    Thread.yield();
                    image.free();
                    if (targets.length == 0 || targets[0].m_score < kVisionThreshold) {
                        getWatchdog().feed();
                        System.out.println("No target found");
                        Target[] newTargets = new Target[targets.length + 1];
                        newTargets[0] = new Target();
                        newTargets[0].m_majorRadius = 0;
                        newTargets[0].m_minorRadius = 0;
                        newTargets[0].m_score = 0;
                        for (int i = 0; i < targets.length; i++) {
                            newTargets[i + 1] = targets[i];
                        }
                        trackerDashboard.updateVisionDashboard(0.0, gyro.getAngle(), 0.0, 0.0, newTargets);
                    } else {
                        getWatchdog().feed();
                        System.out.println(targets[0]);
                        System.out.println("Target Angle: " + targets[0].getHorizontalAngle());
                        trackerDashboard.updateVisionDashboard(0.0, gyro.getAngle(), 0.0, targets[0].m_xPos / targets[0].m_xMax, targets);
                        //may need to get negative of angle
                        targetAngle = targets[0].getHorizontalAngle(); //make target angle Left - Right +
                        System.out.println("Gyro Angle: " + gyroAngle);
                        gyroFinalAngle = gyroAngle + targetAngle;
                        System.out.println("Gyro File Angle: " + gyroAngle);
                        if (targetAngle > 0) {
                            drivetrain.setLeftRightMotorSpeeds(-.15, .15);
                            while (gyro.getAngle() < gyroFinalAngle && !leftStick.getRawButton(5)) {
                                 getWatchdog().feed();
                            }
                            drivetrain.setLeftRightMotorSpeeds(0, 0);
                        } else {
                            drivetrain.setLeftRightMotorSpeeds(.15, -.15);
                            while (gyro.getAngle() > gyroFinalAngle && !leftStick.getRawButton(5)) {
                                  getWatchdog().feed();
                            }
                            drivetrain.setLeftRightMotorSpeeds(0, 0);
                        }
                    }
                }
            } catch (NIVisionException ex) {
                ex.printStackTrace();
            } catch (AxisCameraException ex) {
                ex.printStackTrace();
            }
        }
    }

    /**
     * Gets the left encoder object.
     * @return The left encoder
     */
    public Encoder getLeftEncoder() {
        return leftEncoder;
    }

    /**
     * Gets the right encoder object.
     * @return The right encoder
     */
    public Encoder getRightEncoder() {
        return rightEncoder;
    }

    public Gyro getGyro() {
        return gyro;
    }
}
